import vectorfield
import rootfind
import parserbasic
import copy
import random
import domain
from domainparser import DelayedDomain

JBN_DELTA = 0.001
COMPUTE_DISCRETE_DOMAINS = False

class SampleCounter:
    """Helper class for iteration-constrained sampling.  Raises
    a StopIteration() after a breakpoint is achieved"""
    def __init__(self,initVal=0,breakpoint=-1):
        self.value = initVal
        self.breakpoint = breakpoint
    def __iadd__(self,n):
        self.value += n
        if n > 0 and self.breakpoint >= 0 and self.value >= self.breakpoint:
            raise StopIteration()
        return self


class Precondition:
    """A precondition consists of a piece of code whose scope is the
    global scope + parameter values + state variables"""
    def __init__(self):
        self.type = 'normal'
        self.code = 'True'
        #These are the parameters upon which the precondition is defined
        self.paramdeps = []
        #These are the state variables upon which the precondition is defined
        self.statedeps = []

    def __str__(self):
        if self.type == 'normal':
            return self.code
        elif self.type == 'equality':
            return '|'+self.code+'| < '+str(self.tol)
        elif self.type == 'planning':
            return 'MP '+self.code
        else:
            return 'Invalid precondition'

    def setCode(self,code,env,stateNames,paramNames):
        self.type = 'normal'
        self.code = code
        tokens = set(parserbasic.tokenize_code(self.code))
        self.statedeps = set(stateNames) & tokens
        self.paramdeps = set(paramNames) & tokens

    def setEquality(self,code,env,stateNames,paramNames,tol=1e-4,jacobians={}):
        self.type = 'equality'
        self.tol = tol
        self.code = code
        self.jacobians = jacobians
        tokens = set(parserbasic.tokenize_code(self.code))
        self.statedeps = set(stateNames) & tokens
        self.paramdeps = set(paramNames) & tokens
        #sanity check
        for jparam in self.jacobians:
            if jparam not in paramNames:
                raise ValueError("Invalid jacobian parameter specified")
            if jparam not in self.paramdeps:
                raise ValueError("Jacobian parameter specified that does not appear in the expression")

    def setMotionPlanning(self,code):
        self.type = 'planning'
        self.code = code

    def applicable(self,env,parameters,state):
        """Returns true if this precondition holds with the given parameters
        and state"""
        localvars = state.copy()
        localvars.update(parameters)
        localvars['__state__'] = state
        return self.applicableEnv(env,localvars)

    def applicableEnv(self,env,localvars):
        """Returns true if this precondition holds with the given local env
        (parameters and state)."""
        if self.type == 'normal':
            return True == eval(self.code,env,localvars)
        elif self.type == 'equality':
            res = eval(self.code,env,localvars)
            if not hasattr(res,'__iter__'):
                return abs(res) <= self.tol
            else:
                return all(abs(r)<=self.tol for r in res)
        else:
            return True

class Action:
    """An action description consists of a name, a dictionary of
    parameters->domains, a list of vector field preconditions, a list of
    other preconditions, and a set of effects.

    Optionally, a cost function may be implemented.  This function consists
    of a list of terms that are summed together.  Each term may be
    constant, or state/parameter dependent.  The cost() method evaluates the
    sum of as many terms as can be evaluated given the inputted params/state.

    An action, instantiated with parameter values, can be applied to a
    state if all of its preconditions are met.
    """
    def __init__(self, name):
        self.name = name
        self.params = dict()
        self.preconds = []
        self.effects = []
        self.costTerms = []

    def addParam(self,name,domain):
        self.params[name]=domain

    def paramNames(self):
        return [x for x in params]

    def isParamContinuous(self,p):
        return self.params[p].dimensionality()>0

    def continuousParams(self):
        return [p for (p,d) in self.params.iteritems() if not isinstance(d,DelayedDomain) and d.dimensionality()>0]

    def discreteParams(self):
        return [p for (p,d) in self.params.iteritems() if not isinstance(d,DelayedDomain) and d.dimensionality()==0]

    def delayParams(self):
        return [p for (p,d) in self.params.iteritems() if isinstance(d,DelayedDomain)]

    def addPrecond(self,precond):
        self.preconds.append(precond)

    def addEffect(self,effect):
        self.effects.append(effect)

    def normalPreconds(self):
        return [p for p in self.preconds if p.type == 'normal']

    def equalityPreconds(self):
        return [p for p in self.preconds if p.type == 'equality']

    def planningPreconds(self):
        return [p for p in self.preconds if p.type == 'planning']

    def stateInvariantPreconds(self):
        return [p for p in self.preconds if len(p.statedeps)==0]

    def paramInvariantPreconds(self):
        return [p for p in self.preconds if len(p.paramdeps)==0]
   
    def applicable(self,env,parameters=None,state=None):
        """Tests applicability of the action to the given parameters and/or state.
        If parameters is not given, this only tests state-dependent preconditions.
        If state is not given, this only tests state-independent preconditions."""
        if parameters != None:
            #check parameter domains
            for (p,val) in parameters.items():
                d = self.params[p]
                if isinstance(d,DelayedDomain):
                    newEnv = env.copy()
                    if parameters: newEnv.update(parameters)
                    if state: newEnv.update(state)
                    d.bind(newEnv)
                    if not d.object.contains(val):
                        return False
                    d.object = None
                else:
                    if not d.contains(val):
                        return False
        #check preconditions
        if parameters==None:
            for p in self.paramInvariantPreconds():
                if not p.applicable(env=env,state=state,parameters={}):
                    return False;
        elif state==None:
            for p in self.stateInvariantPreconds():
                if not p.applicable(env=env,state={},parameters=parameters):
                    return False;
        else:
            for p in self.preconds:
                if not p.applicable(env=env,state=state,parameters=parameters):
                    return False;
        return True
        
    def apply(self, env, parameters, state):
        """Applies the action, instantiated with the given parameters,
        on the given state.  Returns the resulting state.
        """
        assert(self.applicable(env,parameters,state))
        localvars = copy.deepcopy(state)
        localvars.update(copy.deepcopy(parameters))
        for e in self.effects:
            exec e in env, localvars
        res = {}
        for x in state:
            res[x]=localvars[x]
        return res

    def evalTermPartial(self,term,env,localvars):
        if isinstance(term,(int,float)):
            return term
        elif isinstance(term,str):
            #evaluate the term, if possible
            try:
                return eval(term,env,localvars)
            except NameError as e:
                #assume it's because the expression depends on unprovded arguments
                return 0
        else:
            raise RuntimeError("cost expression has unknown type, must be numeric or string expression")

    def cost(self, env, params=None, state=None):
        if not self.costTerms:
            return 1
        sumcost = 0
        localvars = {}
        if state: localvars.update(state)
        if params: localvars.update(params)
        for c in self.costTerms:
            sumcost += self.evalTermPartial(c,env,localvars)
        return sumcost


class BoundAction:
    """An action that has bound parameters"""
    def __init__(self,action,params):
        self.action = action
        self.params = params

    def __repr__(self):
        s = self.action.name+"("
        pstrings = [p+'='+repr(val) for (p,val) in self.params.items()]
        s += ','.join(pstrings)+')'
        return s

    def applicable(self,env,state):
        return self.action.applicable(env,self.params,state)

    def apply(self,env,state):
        return self.action.apply(env,self.params,state)

    def cost(self,env,state=None):
        return self.action.cost(env,self.params,state)


class NamedVariablePacking:
    def __init__(self,nameDomains):
        """Takes a dictionary mapping name->DomainInterface"""
        self.nameDomains = nameDomains

    def size(self):
        return sum([d.dimensionality() for (n,d) in self.nameDomains.items()])

    def unpack(self,x):
        d = {}
        c = 0
        for (n,dom) in self.nameDomains.items():
            e = dom.dimensionality()
            if e == 1:
                d[n] = x[c]
            else:
                d[n] = list(x[c:c+e])
            c += e
        return d

    def pack(self,d):
        x = []
        for (n,dom) in self.nameDomains.items():
            if isinstance(d[n],float) or isinstance(d[n],int):
                x.append(d[n])
            else:
                x += d[n]
        return x

    def ranges(self):
        r = {}
        c = 0
        for (n,dom) in self.nameDomains.items():
            e = dom.dimensionality()
            if e == 1:
                r[n]=(c,c+1)
            else:
                r[n]=(c,c+e)
            c += e
        return r

class ActionVectorField:
    def __init__(self, fnCode, fnParams, env, packing):
        self.fnCode = fnCode
        self.fnParams = fnParams
        self.packing = packing
        self.env = env
        self.locals = {}
        self.jacobians = {}

    def numDimensions(self):
        x = [0.0]*self.packing.size()
        return len(self.eval(x))
      
    def eval(self, x):
        self.locals.update(self.packing.unpack(x))
        return self.evalLocal()

    def evalLocal(self):
        try:
            res = eval(self.fnCode, self.env, self.locals)
        except:
            print 'Error executing ',self.fnCode
            raise
        if isinstance(res, int) or isinstance(res, float):
            return [res]
        else:
            return res
        
    def jacobian(self, x):
        m = self.numDimensions()
        self.locals.update(self.packing.unpack(x))
        ranges = self.packing.ranges()
        J = []
        for p in ranges:
            a,b = ranges[p]
            if p in self.fnParams:
                Jp = None
                if p in self.jacobians:
                    try:
                        Jp = eval(self.jacobians[p],self.env,self.locals)
                    except:
                        print 'Error executing jacobian code',self.jacobians[p]
                        raise
                else:
                    Jp = self.jacobianNumeric(p,JBN_DELTA)

                if isinstance(self.locals[p],(float,int)):
                    assert(b-a==1)
                    if m == 1:
                        if isinstance(Jp,(float,int)):
                            Jp = [Jp]
                    assert(len(Jp) == m)
                    J.append(Jp)
                else:
                    assert(len(Jp) == b-a)
                    if m == 1:
                        #convert vectors to n x 1 matrices
                        for i,Jpi in enumerate(Jp):
                            if isinstance(Jpi,(float,int)):
                                Jp[i] = [Jpi]
                    assert(all((len(Jpi) == m) for Jpi in Jp))
                    J.extend(Jp)
            else:
                #append empty rows because they're not being used
                for i in xrange(a,b):
                    J.append([0.0]*m)
        assert(len(J) == self.packing.size())
        #return the transpose so that J[i][j] is the jacobian matrix entry
        return zip(*J)


    def jacobianNumeric(self,p,delta):
        """Returns a numeric approximation of the jacobian with respect to
        the parameter p.  Uses the centered differencing method.

        This actually returns the transpose of the jacobian."""
        orig = self.locals[p]
        if isinstance(orig,(float,int)):
            #Add x = x+delta
            self.locals[p] = orig - delta
            e1 = self.evalLocal()

            #Add x = x+delta
            self.locals[p] = orig + delta
            e2 = self.evalLocal()

            #set x = x (original)
            self.locals[p] = orig
            # place (e2-e1)/(2*delta) in the i'th row
            Jp=[(ei2-ei1)/(2.0*delta) for (ei1,ei2) in zip(e1,e2)]
            return Jp
        else:
            #need a copy
            orig = orig[:]
            Jp = []
            for i in xrange(len(orig)):
                #Add x[i] = x[i]-delta
                self.locals[p][i] = orig[i] - delta
                e1 = self.evalLocal()
                
                #Add x[i] = x[i]+delta
                self.locals[p][i] = orig[i] + delta
                e2 = self.evalLocal()

                #set x[i] = x[i] (original)
                self.locals[p][i] = orig[i]

                # place (e2-e1)/(2*delta) in the i'th row
                Jp.append([(ei2-ei1)/(2.0*delta) for (ei1,ei2) in zip(e1,e2)])
            return Jp
        
class CompositeActionVectorFieldFunction(vectorfield.VectorFieldFunction):
    def __init__(self, fns, fnParams, params, env):
        # fns a list of equality Preconditions
        # params a dict of name->domain pairs for action parameters
        #  used by the VF

        #assert all parameters are continuous
        assert(all(d.dimensionality()>=1 for (p,d) in params.items()))
        #assert all fns are equality Precondition's
        assert(all(f.type == 'equality' for f in fns))
               
        self.packing = NamedVariablePacking(params)
        self.fns = []
        for (fn,fnParam) in zip(fns,fnParams):
            vf = ActionVectorField(fn.code,fnParam,env,self.packing)
            vf.jacobians = fn.jacobians
            self.fns.append(vf)
        self.params = params

        self.n = self.packing.size()
        self.m = None
        self.ranges = None

    def bind(self,localVars):
        localCopy = localVars.copy()
        for fn in self.fns:
            fn.locals = localCopy
        self.m = 0
        self.ranges = []
        for fn in self.fns:
            d = fn.numDimensions()
            self.ranges.append((self.m,self.m+d))
            self.m += d
        
    def bounds(self):
        b = []
        for (p,d) in self.params.items():
            db = d.bounds()
            if isinstance(db,tuple):
                b.append(db)
            else:
                b.extend(db)
        return b
        
    def fn_idx(self, i):
        c = 0
        for fn in self.fns:
            if i < fn.numDimensions():
                return c
            i -= fn.numDimensions()
            c += 1
        raise RuntimeError("Function index out of bounds")

    def eval(self, x):
        res = []
        for fn in self.fns:
            res.extend(fn.eval(x));
        return res
        
    def eval_i(self, x, i):
        f = self.fn_idx(i)
        return self.fns[f].eval(x)[i-self.ranges[f][0]]

    def jacobian(self, x):
        res = []
        for fn in self.fns:
            res.extend(fn.jacobian(x));
        return res
    
    # j needs to be adjusted for changes in indexing to sub-functions
    def jacobian_ij(self, x, i, j):
        f = self.fn_idx(i)
        return self.fns[f].jacobian(x)[i-self.ranges[f][0]][j]


def checkJacobianNumeric(f,x,deltax=1e-4):
    """Returns the max error between the jacobian f.jacobian(x) and the
    numerical differentiation estimate"""
    temp = x[:]
    Jn = []
    for i in xrange(len(x)):
        temp[i] = x[i]-deltax
        e1 = f.eval(temp)
        temp[i] = x[i]+deltax
        e2 = f.eval(temp)
        Jn.append([(ei2-ei1)/(2.0*deltax) for (ei1,ei2) in zip(e1,e2)])
    Jn = zip(*Jn)
    J = f.jacobian(x)
    #print "x",x
    #print 'Numeric:'
    #print Jn
    #print 'f.jacobian()'
    #print J
    err = 0
    for i in xrange(len(J)):
        assert len(J[i])==len(x)
        assert len(Jn[i])==len(x)
        err = max(err,max(Jij-Jnij for (Jij,Jnij) in zip(J[i],Jn[i])))
    return err

class ActionParameterSampler:
    """A class to assist planners in sampling of feasible parameters for
    actions.  Uses root-finding to treat equality preconditions.
    """
    def __init__(self,action,env):
        self.action = action
        #This is a temporary environment used to store state variables
        self.env = env.copy()
        self.rootSolveIters = 100
        self.vectorField = None
        self.numResolved = 0
        self.numResolveFailed = 0

    def bindState(self,state):
        """This function must be called before trying to sample any actions
        on a state."""
        self.env.update(state)
        self.env['__state__'] = state
        # Handle delayed evals
        self.domains = {}
        for (p,d) in self.action.params.items():
            if isinstance(d, DelayedDomain):
                if all(dep in state for dep in d.deps):
                    #only state dependent
                    d.bind(self.env)
                    self.domains[p] = d.object
                    d.object = None
                else:
                    self.domains[p] = d
            else:
                self.domains[p] = d        

        if self.vectorField != None:
            #not the first time setup...
            if COMPUTE_DISCRETE_DOMAINS:
                self.update_domains(1000)
        else:
            #first time setup -- setup up some state-free structures
            self.setup_precondition_cache()
            if COMPUTE_DISCRETE_DOMAINS:
                self.setup_domains()
                self.discreteDomainsStatefree = self.discreteDomains.copy()

    def setup_precondition_cache(self):
        #Set up some basic cached information
        self.normalParams = set(self.domains.keys())
        self.vectorParams = set([])
        for pre in self.action.preconds:
            if pre.type == 'equality':
                for p in pre.paramdeps:
                    if self.domains[p].dimensionality()>=1:
                        self.vectorParams.add(p)
                        self.normalParams.discard(p)
        self.normalOnlyPreconds = []
        self.vectorPreconds = []
        self.normalAndVectorPreconds = []
        for pre in self.action.preconds:
            if pre.type == 'normal':
                if all(p in self.normalParams for p in pre.paramdeps):
                    self.normalOnlyPreconds.append(pre)
                else:
                    self.normalAndVectorPreconds.append(pre)
            elif pre.type == 'equality':
                self.vectorPreconds.append(pre)

        #Set up the vector field function among continuous vector params
        vectorDomains = dict((p,self.action.params[p]) for p in self.vectorParams)
        vectorDeps = [pre.paramdeps for pre in self.action.preconds if pre.type == 'equality']
        self.vectorField = CompositeActionVectorFieldFunction(self.vectorPreconds,vectorDeps,vectorDomains,self.env)

    def enumerate_discrete_domains(self,thresh=10000):
        """Enumerate all applicable discrete parameter settings, and spits
        them out as a generator.  Assumes the state is bound.  Does not
        require any setup_domains or update_domains calls.
        """
        #check state dependent preconditions
        for pre in self.normalOnlyPreconds:
            if len(pre.paramdeps)==0:
                if not pre.applicableEnv(self.env,{}):
                    return

        assignments = None
        order = []
        for p in self.normalParams:
            d = self.domains[p]
            if hasattr(d,'__len__'):
                #domain is enumerable
                order.append(p)
                values = list(d)
                if assignments == None:
                    assignments = [(v,) for v in values]
                else:
                    assignments = [vd+(v,) for vd in assignments for v in values]
        #enumerate delay domains
        for p in self.normalParams:
            d = self.domains[p]
            if isinstance(d,DelayedDomain):
                assert( all(dep in stateAndParams for dep in d.deps)),"We haven't implemented nested domain dependencies yet"
                order.append(p)
            
                stateAndParams = self.env['__state__'].copy()
                if assignments == None:
                    d.bind(stateAndParams)
                    assert(hasattr(d.object,'__len__'))
                    assignments = [(v,) for v in d.object]
                else:
                    for values in assignments:
                        stateAndParams.update(dict(zip(order,values)))
                        d.bind(stateAndParams)
                        assert(hasattr(d.object,'__len__'))
                        assignments = [values+(v,) for v in d.object]
                d.object = None
                
        #look at all parameter settings
        if assignments == None:
            yield {}
            return
        enumerated_names = set(order)
        for values in assignments:
            params=dict(zip(order,values))
            applicable = True
            for pre in self.normalOnlyPreconds:
                if all(dep in enumerated_names for dep in pre.paramdeps):
                    if not pre.applicableEnv(self.env,params):
                        applicable = False
                        break
            if applicable:
                yield params

    def setup_domains(self,thresh=100,enumThresh=10000):
        #extract discrete state-independent domains
        self.discreteDomains = {}
        for p in self.normalParams:
            d = self.domains[p]
            if hasattr(d,'__len__'):
                if len(d) < thresh:
                    self.discreteDomains[p] = list(d)
                    
        #reduce the domains using the state-free preconditions
        for pre in self.normalOnlyPreconds:
            if len(pre.statedeps)==0 and all(dep in self.discreteDomains for dep in pre.paramdeps):
                self.reduce_domains(pre,enumThresh)

    def update_domains(self,thresh=1000):
        """Reduces the discreteDomains member based on the current
        environment and the current normal-only preconditions"""
        firstTime = True
        for pre in self.normalOnlyPreconds:
            if len(pre.statedeps) > 0 and all(dep in self.discreteDomains for dep in pre.paramdeps):
                if firstTime:
                    print "Constructing discrete domains..."
                    self.discreteDomains = copy.deepcopy(self.discreteDomainsStatefree)
                    firstTime = False
                self.reduce_domains(pre,thresh)
        #print self.discreteDomains

    def reduce_domains(self,pre,thresh=100000):
        """Reduces the discreteDomains member based on the precondition pre
        and the current environment"""
        if len(pre.paramdeps)==0:
            return
        total = reduce(lambda x,y:x*y, [len(self.discreteDomains[dep]) for dep in pre.paramdeps])
        #check if there are too many items
        if total >= thresh: return
        #enumerate all domains, and spit them out
        d = None
        for dep in pre.paramdeps:
            if d == None:
                d = [(v,) for v in self.discreteDomains[dep]]
            else:
                d = [vd+(v,) for vd in d for v in self.discreteDomains[dep]]
        feas = dict((dep,set([])) for dep in pre.paramdeps)
        for values in d:
            params = dict(zip(pre.paramdeps,values))
            if pre.applicableEnv(self.env,params):
                for p,v in zip(pre.paramdeps,values):
                    feas[p].add(v)
        for (p,v) in feas.iteritems():
            self.discreteDomains[p] = list(v)

    def sample_domain(self,p,counter):
        """Samples a value of parameter p in its domain."""

        if hasattr(self,'discreteDomains') and p in self.discreteDomains:
            counter += 1
            return random.choice(self.discreteDomains[p])
        while True:
            #consume iterations until a feasible value is sampled
            res = self.domains[p].sample()
            counter += 1
            if self.domains[p].contains(res):
                return res
        #guaranteed never to reach this point
        return None

                
    def sample_domains(self,counter,subset=None,givens=None):
        """Samples a set of parameters from their domains.

        If subset is provided, only samples from the subset of parameters.
        Otherwise, self.normalParams is used.

        If givens is provided, the parameter dict is initialized by givens.
        
        Resulting dict ensures that the parameters are in their domains.
        If the domains are dependent, then a greater counter breakpoint
        is not necessarily helpful.  Multiple calls of sample_domains may
        be needed.
        """
        if subset==None:
            subset = self.normalParams
        params = {}
        if givens: params = givens.copy()
        if any(isinstance(self.domains[p],DelayedDomain) for p in subset):
            #more complex solution technique for domain interdependencies
            tempdomains = {}
            for p in subset:
                d = self.domains[p]
                if not isinstance(d,DelayedDomain):
                    params[p] = self.sample_domain(p,counter)
                else:
                    tempdomains[p] = d
            resolved = set(self.env['__state__'].keys()) | set(params.keys())
            unresolved = set(p for p in subset if isinstance(self.domains[p],DelayedDomain))
            newenv = self.env.copy()
            newenv.update(params)
            #print "Resolving unresolved domains..."
            #print "Resolved:",list(resolved)
            #print "Unresolved:",list(unresolved)
            while len(unresolved) > 0:
                changed = 0
                for p in list(unresolved):
                    d = tempdomains[p]
                    if isinstance(d,DelayedDomain):
                        #check if we can bind it now
                        if all(dep in resolved for dep in d.deps):
                            changed += 1
                            d.bind(newenv)
                            tempdomains[p] = d.object
                    else:
                        #finally sample the domain
                        changed += 1
                        unresolved.remove(p)
                        resolved.add(p)
                        while True:
                            #consume iterations until a feasible value is sampled
                            params[p] = d.sample()
                            counter += 1
                            if d.contains(params[p]):
                                break
                        newenv[p] = params[p]
                if changed == 0:
                    print "Remaining parameters to be sampled:",' '.join(list(unresolved))
                    raise RuntimeError("Cyclic reference in parameter domains?")
        else:
            #simple solution
            for p in subset:
                params[p] = self.sample_domain(p,counter)
        return params

    def self_test_jacobian(self,state,deltax=1e-4):
        self.bindState(state)
        params = {}
        for p in self.domains:
            params[p] = self.domains[p].sample()
        self.vectorField.bind(params)
        start = self.vectorField.packing.pack(params)
        print 'Jacobian errors:'
        for fn in self.vectorField.fns:
            err = checkJacobianNumeric(fn,start,deltax)
            print fn.fnCode,':',err

    def resolve_vectorfields(self,parameters):
        """Given a set of input parameters as a starting value, this method
        tries to solve all continuous vectorfunctions so that the preconditions
        are satisfied.  The final parameter setting is given by modifying the
        parameters structure.

        Returns True on success.
        """
        #print 'Optimizing parameters',parameters
        composite = self.vectorField
        #binds all non-optimized parameters, e.g., discrete values
        composite.bind(parameters)
        #Sets the starting vector and the vector bounds
        start = composite.packing.pack(parameters)
        #print 'Start vector',start
        bounds = composite.bounds()
        #print 'Bounds',bounds
        if len(start) != composite.num_vars():
            raise RuntimeError("Start mismatch: " + str(len(start)) + " " +
                                str(composite.num_vars()))
        if len(bounds) != composite.num_vars():
            raise RuntimeError("Bounds mismatch: " + str(len(bounds)) + " " +
                                str(composite.num_vars()))
        rootfind.setVectorField(composite)
        #set the desired tolerance
        tol = min(p.tol for p in self.vectorPreconds)
        rootfind.setFTolerance(tol)
        #run the root finder
        code,roots,iters = rootfind.findRootsBounded(start, bounds, self.rootSolveIters)
        rootfind.destroy()
        if code == 1:
            # Success on f convergence
            parameters.update(composite.packing.unpack(roots))
            self.numResolved += 1
            return True
        # Fail
        self.numResolveFailed += 1
        print "Result:",composite.eval(roots),"code",code
        print "Start value:",composite.eval(start)
        return False

    def sample_normal(self,maxIters,subset=None,givens=None):
        """Samples just the parameters specified in subset.  They should all be
        normal (non-vectorfield) parameters.
        Return value is a triple (s,n,p) where s is true if successful,
        n is the number of iterations consumed, and p is the parameter dict.
        
        This algorithm is a naive sampling that samples all domains then
        tests all preconditions.
        """
        if subset == None:
            subset = self.normalParams
        pMaxIters = maxIters*len(subset)
        counter = SampleCounter(breakpoint=pMaxIters)
        params = None
        try:
            while True:
                #generate initial sample
                dcounter = SampleCounter(breakpoint=len(subset)*2)
                try:
                    params = self.sample_domains(dcounter,subset=subset,givens=givens)
                    counter += dcounter.value
                except StopIteration:
                    counter += dcounter.value
                    continue
                #test preconditions
                applicable = True
                for pre in self.normalOnlyPreconds:
                    if not pre.applicableEnv(self.env,params):
                        applicable=False
                        break
                if applicable:
                    return True,counter.value/len(self.normalParams),params
                #following line is to address problem: no domains, not applicable
                if dcounter.value == 0:
                    counter += 1
        except StopIteration:
            return False,maxIters,None

    def sample_normal_walk(self,maxIters,subset=None,givens=None):
        """Samples just the parameters specified in subset.  They should all be
        normal (non-vectorfield) parameters.
        Return value is a triple (s,n,p) where s is true if successful,
        n is the number of iterations consumed, and p is the parameter dict.

        This algorithm uses a walkSAT-like approach.
        1. sample domains
        2. test conditions
        3. pick a variable involved in a failed condition (heuristic: use the
           one involved in the most failures)
        4. try values for that variable until it makes the condition
           successful, or for some short number of iterations
        5. repeat 3-4 until completion
        """
        if subset==None:
            subset = self.normalParams
        if len(subset)==0: return True,0
        pMaxIters = maxIters*len(subset)
        counter = SampleCounter(breakpoint=pMaxIters)
        try:
            #counter will call StopIteration when pMaxIters is reached
            while True:
                dcounter = SampleCounter(breakpoint=len(subset)*2)
                try:
                    params = self.sample_domains(dcounter,subset=subset,givens=givens)
                    counter += dcounter.value
                except StopIteration:
                    counter += dcounter.value
                    continue

                #print 'Initial sample',params
                #Start the descent iterations
                for descentIters in xrange(len(subset)*2):
                    numFailed = dict((p,0) for p in self.normalParams)
                    failedPre = dict((p,None) for p in self.normalParams)
                    for pre in self.normalOnlyPreconds:
                        if not pre.applicableEnv(self.env,params):
                            for dep in pre.paramdeps:
                                numFailed[dep] += 1
                                #a clever way of making failedPre[dep] be a randomly selected failed precondition...
                                if random.random()*numFailed[dep] <= 1.0:
                                    failedPre[dep] = pre
                    fmax,pworst = max((numFailed[p],p) for p in subset)
                    #print pworst,'failed',fmax,'conditions, including',failedPre[pworst]
                    if fmax == 0:
                        #done
                        return True,counter.value/len(subset),params

                    #sample pworst until it's not the same as before
                    porig = copy.deepcopy(params[pworst])
                    resampleIters = 0
                    while resampleIters < len(subset)*2:
                        #consume iterations on just this parameter
                        params[pworst] = self.sample_domain(pworst,counter)
                        if params[pworst] != porig:
                            if failedPre[pworst].applicableEnv(self.env,params):
                                #must pass one of the failed preconditions
                                #print 'got successful',pworst,'=',params[pworst]
                                break
                        resampleIters += 1
                    if resampleIters == len(subset)*2:
                        break
                    #otherwise, continue with a new paramter
        except StopIteration:
            #counter reached maximum
            return False,maxIters,None

    def sample_applicable_vectorfield(self,params,maxIters):
        """Internal use only -- given an assignment of valid non-vectorfield
        parameters, finds the vectorfield parameters using up to maxIters
        iterations."""
        #no root finding necessary
        if len(self.vectorParams)==0: return params

        #do root finding
        iters = 0
        while iters < maxIters:
            #Sample vector parameters
            for p in self.vectorParams:
                params[p] = self.domains[p].sample()
            #print "parameter settings...",params
            if self.resolve_vectorfields(params):
                #print "Resolved"
                #test the normal preconds that are dependent on these params
                applicable = True
                for p in self.vectorParams:
                    if not self.domains[p].contains(params[p]):
                        print p,'exited parameter domain'
                        applicable = False
                        break
                for pre in self.vectorPreconds:
                    if not pre.applicableEnv(self.env,params):
                        print "Warning, vector precondition was solved, but not applicable"
                        print pre.code
                        res = eval(pre.code,self.env,params)
                        print "Result = ",res,", tol=",pre.tol
                        applicable = False
                        break
                for pre in self.normalAndVectorPreconds:
                    if not pre.applicableEnv(self.env,params):
                        #print 'failed precondition',pre.code
                        applicable = False
                        break
                if applicable:
                    return params
            else:
                print "Not resolved, action",self.action.name,'params',params
            iters += 1
        return None        

    def sample_applicable(self,maxIters):
        """
        Samples a set of applicable parameters from scratch, using at most
        maxIters iterations.  Returns None on failure, otherwise returns
        a dictionary of applicable parameters.
        """        
        #just sample the normal-only parameters
        #applicable,iters,params = self.sample_normal_walk(maxIters)
        #print 'sample_normal_walk iters',applicable,iters
        applicable,iters,params = self.sample_normal(maxIters)
        #print 'sample_normal iters',applicable,iters

        #couldn't find a set of non-equality parameters that satisfied
        #the preconditions
        if not applicable: return None

        return self.sample_applicable_vectorfield(params,maxIters-iters)

    def sample_applicable_partial(self,fixedParams,maxIters):
        """
        Samples a set of applicable parameters from a given partial assignment,
        using at most maxIters iterations.  Returns None on failure,
        otherwise returns a dictionary of applicable parameters.

        Currently params may not include vector field parameters.
        """
        freeParams = list(set(self.normalParams)-set(fixedParams))
        #just sample the normal-only parameters
        #applicable,iters,params = self.sample_normal_walk(maxIters,subset=freeParams,givens=fixedParams)
        #print 'sample_normal_walk iters',applicable,iters
        applicable,iters,params = self.sample_normal(maxIters,subset=freeParams,givens=fixedParams)
        #print 'sample_normal iters',applicable,iters

        #couldn't find a set of non-equality parameters that satisfied
        #the preconditions
        if not applicable: return None

        #no root finding necessary
        if len(self.vectorParams)==0: return params
        
        if any([p in self.vectorParams for p in params]):
            raise RuntimeError("Partial parameter assignment includes vector params")

        return self.sample_applicable_vectorfield(params,maxIters-iters)
